fusion.c File Reference

Include dependency graph for fusion.c:

Go to the source code of this file.

Functions
void	fInitializeFusion (SensorFusionGlobals *sfg)
void	fFuseSensors (struct SV_1DOF_P_BASIC *pthisSV_1DOF_P_BASIC, struct SV_3DOF_G_BASIC *pthisSV_3DOF_G_BASIC, struct SV_3DOF_B_BASIC *pthisSV_3DOF_B_BASIC, struct SV_3DOF_Y_BASIC *pthisSV_3DOF_Y_BASIC, struct SV_6DOF_GB_BASIC *pthisSV_6DOF_GB_BASIC, struct SV_6DOF_GY_KALMAN *pthisSV_6DOF_GY_KALMAN, struct SV_9DOF_GBY_KALMAN *pthisSV_9DOF_GBY_KALMAN, struct AccelSensor *pthisAccel, struct MagSensor *pthisMag, struct GyroSensor *pthisGyro, struct PressureSensor *pthisPressure, struct MagCalibration *pthisMagCal)
void	fInit_1DOF_P_BASIC (struct SV_1DOF_P_BASIC *pthisSV, struct PressureSensor *pthisPressure, float flpftimesecs)
void	fInit_3DOF_G_BASIC (struct SV_3DOF_G_BASIC *pthisSV, struct AccelSensor *pthisAccel, float flpftimesecs)
void	fInit_3DOF_B_BASIC (struct SV_3DOF_B_BASIC *pthisSV, struct MagSensor *pthisMag, float flpftimesecs)
void	fInit_3DOF_Y_BASIC (struct SV_3DOF_Y_BASIC *pthisSV)
void	fInit_6DOF_GB_BASIC (struct SV_6DOF_GB_BASIC *pthisSV, struct AccelSensor *pthisAccel, struct MagSensor *pthisMag, float flpftimesecs)
void	fInit_6DOF_GY_KALMAN (struct SV_6DOF_GY_KALMAN *pthisSV, struct AccelSensor *pthisAccel, struct GyroSensor *pthisGyro)
void	fInit_9DOF_GBY_KALMAN (struct SV_9DOF_GBY_KALMAN *pthisSV, struct AccelSensor *pthisAccel, struct MagSensor *pthisMag, struct GyroSensor *pthisGyro, struct MagCalibration *pthisMagCal)
void	fRun_1DOF_P_BASIC (struct SV_1DOF_P_BASIC *pthisSV, struct PressureSensor *pthisPressure)
void	fRun_3DOF_G_BASIC (struct SV_3DOF_G_BASIC *pthisSV, struct AccelSensor *pthisAccel)
void	fRun_3DOF_B_BASIC (struct SV_3DOF_B_BASIC *pthisSV, struct MagSensor *pthisMag)
void	fRun_3DOF_Y_BASIC (struct SV_3DOF_Y_BASIC *pthisSV, struct GyroSensor *pthisGyro)
void	fRun_6DOF_GB_BASIC (struct SV_6DOF_GB_BASIC *pthisSV, struct MagSensor *pthisMag, struct AccelSensor *pthisAccel)
void	fRun_6DOF_GY_KALMAN (struct SV_6DOF_GY_KALMAN *pthisSV, struct AccelSensor *pthisAccel, struct GyroSensor *pthisGyro)

Function Documentation

void fFuseSensors	(	struct SV_1DOF_P_BASIC *	pthisSV_1DOF_P_BASIC,
		struct SV_3DOF_G_BASIC *	pthisSV_3DOF_G_BASIC,
		struct SV_3DOF_B_BASIC *	pthisSV_3DOF_B_BASIC,
		struct SV_3DOF_Y_BASIC *	pthisSV_3DOF_Y_BASIC,
		struct SV_6DOF_GB_BASIC *	pthisSV_6DOF_GB_BASIC,
		struct SV_6DOF_GY_KALMAN *	pthisSV_6DOF_GY_KALMAN,
		struct SV_9DOF_GBY_KALMAN *	pthisSV_9DOF_GBY_KALMAN,
		struct AccelSensor *	pthisAccel,
		struct MagSensor *	pthisMag,
		struct GyroSensor *	pthisGyro,
		struct PressureSensor *	pthisPressure,
		struct MagCalibration *	pthisMagCal
	)

Definition at line 86 of file fusion.c.

Referenced by runFusion().

{
    // 1DOF Pressure: call the low pass filter algorithm
#if F_1DOF_P_BASIC
    if (pthisSV_1DOF_P_BASIC)
    {
        ARM_systick_start_ticks(&(pthisSV_1DOF_P_BASIC->systick));
        fRun_1DOF_P_BASIC(pthisSV_1DOF_P_BASIC, pthisPressure);
        pthisSV_1DOF_P_BASIC->systick = ARM_systick_elapsed_ticks(pthisSV_1DOF_P_BASIC->systick);
    }
#endif

    // 3DOF Accel Basic: call the tilt algorithm
#if F_3DOF_G_BASIC
    if (pthisSV_3DOF_G_BASIC)
    {
        ARM_systick_start_ticks(&(pthisSV_3DOF_G_BASIC->systick));
        fRun_3DOF_G_BASIC(pthisSV_3DOF_G_BASIC, pthisAccel);
        pthisSV_3DOF_G_BASIC->systick = ARM_systick_elapsed_ticks(pthisSV_3DOF_G_BASIC->systick);
    }
#endif

    // 3DOF Magnetometer Basic: call the 2D vehicle compass algorithm
#if F_3DOF_B_BASIC
    if (pthisSV_3DOF_B_BASIC)
    {
        ARM_systick_start_ticks(&(pthisSV_3DOF_B_BASIC->systick));
        fRun_3DOF_B_BASIC(pthisSV_3DOF_B_BASIC, pthisMag);
        pthisSV_3DOF_B_BASIC->systick = ARM_systick_elapsed_ticks(pthisSV_3DOF_B_BASIC->systick);
    }
#endif

    // 3DOF Gyro Basic: call the gyro integration algorithm
#if F_3DOF_Y_BASIC
    if (pthisSV_3DOF_Y_BASIC)
    {
        ARM_systick_start_ticks(&(pthisSV_3DOF_Y_BASIC->systick));
        fRun_3DOF_Y_BASIC(pthisSV_3DOF_Y_BASIC, pthisGyro);
        pthisSV_3DOF_Y_BASIC->systick = ARM_systick_elapsed_ticks(pthisSV_3DOF_Y_BASIC->systick);
    }
#endif

    // 6DOF Accel / Mag: Basic: call the eCompass orientation algorithm
#if F_6DOF_GB_BASIC
    if (pthisSV_6DOF_GB_BASIC)
    {
        ARM_systick_start_ticks(&(pthisSV_6DOF_GB_BASIC->systick));
        fRun_6DOF_GB_BASIC(pthisSV_6DOF_GB_BASIC, pthisMag, pthisAccel);
        pthisSV_6DOF_GB_BASIC->systick = ARM_systick_elapsed_ticks(pthisSV_6DOF_GB_BASIC->systick);
    }
#endif

    // 6DOF Accel / Gyro: call the Kalman filter orientation algorithm
#if F_6DOF_GY_KALMAN
    if (pthisSV_6DOF_GY_KALMAN)
    {
        ARM_systick_start_ticks(&(pthisSV_6DOF_GY_KALMAN->systick));
        fRun_6DOF_GY_KALMAN(pthisSV_6DOF_GY_KALMAN, pthisAccel, pthisGyro);
        pthisSV_6DOF_GY_KALMAN->systick = ARM_systick_elapsed_ticks(pthisSV_6DOF_GY_KALMAN->systick);
    }
#endif

    // 9DOF Accel / Mag / Gyro: call the Kalman filter orientation algorithm
#if F_9DOF_GBY_KALMAN
    if (pthisSV_9DOF_GBY_KALMAN)
    {
        ARM_systick_start_ticks(&(pthisSV_9DOF_GBY_KALMAN->systick));
        fRun_9DOF_GBY_KALMAN(pthisSV_9DOF_GBY_KALMAN, pthisAccel, pthisMag,
                             pthisGyro, pthisMagCal);
        pthisSV_9DOF_GBY_KALMAN->systick = ARM_systick_elapsed_ticks(pthisSV_9DOF_GBY_KALMAN->systick);
    }
#endif
    return;
}

Here is the call graph for this function:

Here is the caller graph for this function:

void fInit_1DOF_P_BASIC	(	struct SV_1DOF_P_BASIC *	pthisSV,
		struct PressureSensor *	pthisPressure,
		float	flpftimesecs
	)

Definition at line 172 of file fusion.c.

Referenced by fRun_1DOF_P_BASIC().

{
    // set algorithm sampling interval (typically 25Hz) and low pass filter
    // Note: the MPL3115 sensor only updates its output every 1s and is therefore repeatedly oversampled at 25Hz
    // but executing the exponenial filter at the 25Hz rate also performs an interpolation giving smoother output.
    // set algorithm sampling interval (typically 25Hz)
    pthisSV->fdeltat = 1.0F / (float) FUSION_HZ;

    // set low pass filter constant with maximum value 1.0 (all pass) decreasing to 0.0 (increasing low pass)
    if (flpftimesecs > pthisSV->fdeltat)
        pthisSV->flpf = pthisSV->fdeltat / flpftimesecs;
    else
        pthisSV->flpf = 1.0F;

    // initialize the low pass filters to current measurement
    pthisSV->fLPH = pthisPressure->fH;
    pthisSV->fLPT = pthisPressure->fT;

    // clear the reset flag
    pthisSV->resetflag = false;

    return;
}   // end fInit_1DOF_P_BASIC

Here is the caller graph for this function:

void fInit_3DOF_B_BASIC	(	struct SV_3DOF_B_BASIC *	pthisSV,
		struct MagSensor *	pthisMag,
		float	flpftimesecs
	)

Definition at line 228 of file fusion.c.

Referenced by fRun_3DOF_B_BASIC().

{
    // set algorithm sampling interval (typically 25Hz)
    pthisSV->fdeltat = 1.0F / (float) FUSION_HZ;

    // set low pass filter constant with maximum value 1.0 (all pass) decreasing to 0.0 (increasing low pass)
    if (flpftimesecs > pthisSV->fdeltat)
        pthisSV->flpf = pthisSV->fdeltat / flpftimesecs;
    else
        pthisSV->flpf = 1.0F;

    // initialize the low pass filtered magnetometer orientation matrix and quaternion using fBc
#if THISCOORDSYSTEM == NED
    f3DOFMagnetometerMatrixNED(pthisSV->fLPR, pthisMag->fBc);
#elif THISCOORDSYSTEM == ANDROID
    f3DOFMagnetometerMatrixAndroid(pthisSV->fLPR, pthisMag->fBc);
#else // WIN8
    f3DOFMagnetometerMatrixWin8(pthisSV->fLPR, pthisMag->fBc);
#endif
    fQuaternionFromRotationMatrix(pthisSV->fLPR, &(pthisSV->fLPq));

    // update the default quaternion type supported to the most sophisticated
    //if (globals.DefaultQuaternionPacketType < Q3M)
    //globals.QuaternionPacketType = globals.DefaultQuaternionPacketType = Q3M;
    // clear the reset flag
    pthisSV->resetflag = false;

    return;
}   // end fInit_3DOF_B_BASIC

Here is the call graph for this function:

Here is the caller graph for this function:

void fInit_3DOF_G_BASIC	(	struct SV_3DOF_G_BASIC *	pthisSV,
		struct AccelSensor *	pthisAccel,
		float	flpftimesecs
	)

Definition at line 197 of file fusion.c.

Referenced by fRun_3DOF_G_BASIC().

{
    // set algorithm sampling interval (typically 25Hz)
    pthisSV->fdeltat = 1.0F / (float) FUSION_HZ;

    // set low pass filter constant with maximum value 1.0 (all pass) decreasing to 0.0 (increasing low pass)
    if (flpftimesecs > pthisSV->fdeltat)
        pthisSV->flpf = pthisSV->fdeltat / flpftimesecs;
    else
        pthisSV->flpf = 1.0F;

    // apply the tilt estimation algorithm to initialize the low pass orientation matrix and quaternion
#if THISCOORDSYSTEM == NED
    f3DOFTiltNED(pthisSV->fLPR, pthisAccel->fGc);
#elif THISCOORDSYSTEM == ANDROID
    f3DOFTiltAndroid(pthisSV->fLPR, pthisAccel->fGc);
#else // WIN8
    f3DOFTiltWin8(pthisSV->fLPR, pthisAccel->fGc);
#endif
    fQuaternionFromRotationMatrix(pthisSV->fLPR, &(pthisSV->fLPq));

    // update the default quaternion type supported to the most sophisticated
    //if (globals.DefaultQuaternionPacketType < Q3)
    //globals.QuaternionPacketType = globals.DefaultQuaternionPacketType = Q3;
    // clear the reset flag
    pthisSV->resetflag = false;

    return;
}   // end fInit_3DOF_G_BASIC

Here is the call graph for this function:

Here is the caller graph for this function:

void fInit_3DOF_Y_BASIC

(

struct SV_3DOF_Y_BASIC *

pthisSV

)

Definition at line 259 of file fusion.c.

Referenced by fRun_3DOF_Y_BASIC().

{
    // compute the sampling time intervals (secs)
    pthisSV->fdeltat = 1.0F / (float) FUSION_HZ;

    // initialize orientation estimate to flat
    f3x3matrixAeqI(pthisSV->fR);
    fqAeq1(&(pthisSV->fq));

    // update the default quaternion type supported to the most sophisticated
    //if (globals.DefaultQuaternionPacketType < Q3G)
    //globals.QuaternionPacketType = globals.DefaultQuaternionPacketType = Q3G;
    // clear the reset flag
    pthisSV->resetflag = false;

    return;
}   // end fInit_3DOF_Y_BASIC

Here is the call graph for this function:

Here is the caller graph for this function:

void fInit_6DOF_GB_BASIC	(	struct SV_6DOF_GB_BASIC *	pthisSV,
		struct AccelSensor *	pthisAccel,
		struct MagSensor *	pthisMag,
		float	flpftimesecs
	)

Definition at line 277 of file fusion.c.

Referenced by fRun_6DOF_GB_BASIC().

{
    float ftmp;

    // set algorithm sampling interval (typically 25Hz)
    pthisSV->fdeltat = 1.0F / (float) FUSION_HZ;

    // set low pass filter constant with maximum value 1.0 (all pass) decreasing to 0.0 (increasing low pass)
    if (flpftimesecs > pthisSV->fdeltat)
        pthisSV->flpf = pthisSV->fdeltat / flpftimesecs;
    else
        pthisSV->flpf = 1.0F;

    // initialize the instantaneous orientation matrix, inclination angle and quaternion
#if THISCOORDSYSTEM == NED
    feCompassNED(pthisSV->fLPR, &(pthisSV->fLPDelta), &ftmp, &ftmp, pthisMag->fBc, pthisAccel->fGc, &ftmp, &ftmp );
#elif THISCOORDSYSTEM == ANDROID
    feCompassAndroid(pthisSV->fLPR, &(pthisSV->fLPDelta), &ftmp, &ftmp, pthisMag->fBc, pthisAccel->fGc, &ftmp, &ftmp);
#else // WIN8
    feCompassWin8(pthisSV->fLPR, &(pthisSV->fLPDelta), &ftmp, &ftmp, pthisMag->fBc, pthisAccel->fGc, &ftmp, &ftmp);
#endif
    fQuaternionFromRotationMatrix(pthisSV->fLPR, &(pthisSV->fLPq));

    // update the default quaternion type supported to the most sophisticated
    //if (globals.DefaultQuaternionPacketType < Q6MA)
    //    globals.QuaternionPacketType = globals.DefaultQuaternionPacketType = Q6MA;

    // clear the reset flag
    pthisSV->resetflag = false;

    return;
}   // end fInit_6DOF_GB_BASIC

Here is the call graph for this function:

Here is the caller graph for this function:

void fInit_6DOF_GY_KALMAN	(	struct SV_6DOF_GY_KALMAN *	pthisSV,
		struct AccelSensor *	pthisAccel,
		struct GyroSensor *	pthisGyro
	)

Definition at line 313 of file fusion.c.

Referenced by fRun_6DOF_GY_KALMAN().

{
    float   *pFlash;    // pointer to flash float words
    int8    i;          // loop counter

    // compute and store useful product terms to save floating point calculations later
    pthisSV->fdeltat = 1.0F / (float) FUSION_HZ;
    pthisSV->fQwbOver3 = FQWB_6DOF_GY_KALMAN / 3.0F;
    pthisSV->fAlphaOver2 = FPIOVER180 * pthisSV->fdeltat / 2.0F;
    pthisSV->fAlphaSqOver4 = pthisSV->fAlphaOver2 * pthisSV->fAlphaOver2;
    pthisSV->fAlphaQwbOver6 = pthisSV->fAlphaOver2 * pthisSV->fQwbOver3;
    pthisSV->fAlphaSqQvYQwbOver12 = pthisSV->fAlphaSqOver4 * (FQVY_6DOF_GY_KALMAN + FQWB_6DOF_GY_KALMAN) / 3.0F;
    pthisSV->fMaxGyroOffsetChange = sqrtf(fabs(FQWB_9DOF_GBY_KALMAN)) / (float) FUSION_HZ;

    // zero the a posteriori gyro offset and error vectors
    for (i = CHX; i <= CHZ; i++)
    {
        pthisSV->fqgErrPl[i] = 0.0F;
        pthisSV->fbErrPl[i] = 0.0F;
    }

    // check to see if a gyro calibration exists in flash
    // the standard value for erased flash is 0xFF in each byte but for portability check against 0x12345678
    pFlash = (float *) (CALIBRATION_NVM_ADDR + GYRO_NVM_OFFSET);
    if (*((uint32 *) pFlash++) == 0x12345678)
    {
        // copy the gyro calibration from flash into the state vector
        for (i = CHX; i <= CHZ; i++) pthisSV->fbPl[i] = *(pFlash++);
    }
    else
    {
        // set the gyro offset to the current measurement if within limits
        for (i = CHX; i <= CHZ; i++)
        {
            if ((pthisGyro->fYs[i] >= FMIN_6DOF_GY_BPL) &&
                (pthisGyro->fYs[i] <= FMAX_6DOF_GY_BPL))
                pthisSV->fbPl[i] = pthisGyro->fYs[i];
            else
                pthisSV->fbPl[i] = 0.0F;
        }
    }

    // initialize the a posteriori orientation state vector to the tilt orientation
#if THISCOORDSYSTEM == NED
    f3DOFTiltNED(pthisSV->fRPl, pthisAccel->fGc);
#elif THISCOORDSYSTEM == ANDROID
    f3DOFTiltAndroid(pthisSV->fRPl, pthisAccel->fGc);
#else // Win8
    f3DOFTiltWin8(pthisSV->fRPl, pthisAccel->fGc);
#endif
    fQuaternionFromRotationMatrix(pthisSV->fRPl, &(pthisSV->fqPl));

    // update the default quaternion type supported to the most sophisticated
    //if (globals.DefaultQuaternionPacketType < Q6AG)
    //globals.QuaternionPacketType = globals.DefaultQuaternionPacketType = Q6AG;

    // clear the reset flag
    pthisSV->resetflag = false;

    return;
}                       // end fInit_6DOF_GY_KALMAN

Here is the call graph for this function:

Here is the caller graph for this function:

void fInit_9DOF_GBY_KALMAN	(	struct SV_9DOF_GBY_KALMAN *	pthisSV,
		struct AccelSensor *	pthisAccel,
		struct MagSensor *	pthisMag,
		struct GyroSensor *	pthisGyro,
		struct MagCalibration *	pthisMagCal
	)

Definition at line 378 of file fusion.c.

Referenced by fRun_6DOF_GY_KALMAN().

{
    float ftmp;// scratch
    float *pFlash;// pointer to flash float words
    int8 i;// loop counter

    // compute and store useful product terms to save floating point calculations later
    pthisSV->fdeltat = 1.0F / (float) FUSION_HZ;
    pthisSV->fgdeltat = GTOMSEC2 * pthisSV->fdeltat;
    pthisSV->fQwbOver3 = FQWB_9DOF_GBY_KALMAN / 3.0F;
    pthisSV->fAlphaOver2 = FPIOVER180 * pthisSV->fdeltat / 2.0F;
    pthisSV->fAlphaSqOver4 = pthisSV->fAlphaOver2 * pthisSV->fAlphaOver2;
    pthisSV->fAlphaQwbOver6 = pthisSV->fAlphaOver2 * pthisSV->fQwbOver3;
    pthisSV->fAlphaSqQvYQwbOver12 = pthisSV->fAlphaSqOver4 * (FQVY_9DOF_GBY_KALMAN + FQWB_9DOF_GBY_KALMAN) / 3.0F;
    pthisSV->fMaxGyroOffsetChange = sqrtf(fabs(FQWB_9DOF_GBY_KALMAN)) / (float)FUSION_HZ;

    // zero the a posteriori error vectors and inertial outputs
    for (i = CHX; i <= CHZ; i++) {
        pthisSV->fqgErrPl[i] = 0.0F;
        pthisSV->fqmErrPl[i] = 0.0F;
        pthisSV->fbErrPl[i] = 0.0F;
        pthisSV->fVelGl[i] = 0.0F;
        pthisSV->fDisGl[i] = 0.0F;
    }

    // check to see if a gyro calibration exists in flash
    // the standard value for erased flash is 0xFF in each byte but for portability check against 0x12345678
    pFlash = (float *) (CALIBRATION_NVM_ADDR + GYRO_NVM_OFFSET);
    if (*((uint32*) pFlash++) == 0x12345678) {
    // copy the gyro calibration from flash into the state vector
        for (i = CHX; i <= CHZ; i++)
            pthisSV->fbPl[i] = *(pFlash++);
    } else {
        // set the gyro offset to the current measurement if within limits
        for (i = CHX; i <= CHZ; i++) {
            if ((pthisGyro->fYs[i] >= FMIN_9DOF_GBY_BPL) && (pthisGyro->fYs[i] <= FMAX_9DOF_GBY_BPL))
                pthisSV->fbPl[i] = pthisGyro->fYs[i];
            else
                pthisSV->fbPl[i] = 0.0F;
        }
     }

    // initialize the posteriori orientation state vector to the instantaneous eCompass orientation
    pthisSV->iFirstAccelMagLock = false;
#if THISCOORDSYSTEM == NED
    feCompassNED(pthisSV->fRPl, &(pthisSV->fDeltaPl), &(pthisSV->fsinDeltaPl), &(pthisSV->fcosDeltaPl),
        pthisMag->fBc, pthisAccel->fGc, &ftmp, &ftmp);
#elif THISCOORDSYSTEM == ANDROID
    feCompassAndroid(pthisSV->fRPl, &(pthisSV->fDeltaPl),  &(pthisSV->fsinDeltaPl), &(pthisSV->fcosDeltaPl),
        pthisMag->fBc, pthisAccel->fGc, &ftmp, &ftmp);
#else  // WIN8
    feCompassWin8(pthisSV->fRPl, &(pthisSV->fDeltaPl), &(pthisSV->fsinDeltaPl), &(pthisSV->fcosDeltaPl),
        pthisMag->fBc, pthisAccel->fGc, &ftmp, &ftmp);
#endif
    fQuaternionFromRotationMatrix(pthisSV->fRPl, &(pthisSV->fqPl));

    // clear the reset flag
    pthisSV->resetflag = false;

    return;
} // end fInit_9DOF_GBY_KALMAN

Here is the call graph for this function:

Here is the caller graph for this function:

void fInitializeFusion

(

SensorFusionGlobals *

sfg

)

Definition at line 51 of file fusion.c.

Referenced by DecodeCommandBytes(), and initializeFusionEngine().

{
    // reset the quaternion type to the default packet type
    // sfg->pControlSubsystem->QuaternionPacketType = sfg->pControlSubsystem->DefaultQuaternionPacketType;

    // force a reset of all the algorithms next time they execute
    // the initialization will result in the default and current quaternion being set to the most sophisticated
    // algorithm supported by the build
#if F_1DOF_P_BASIC
    sfg->SV_1DOF_P_BASIC.resetflag    = true;
#endif
#if F_3DOF_B_BASIC
    sfg->SV_3DOF_B_BASIC.resetflag    = true;
#endif
#if F_3DOF_G_BASIC
    sfg->SV_3DOF_G_BASIC.resetflag    = true;
#endif
#if F_3DOF_Y_BASIC
    sfg->SV_3DOF_Y_BASIC.resetflag    = true;
#endif
#if F_6DOF_GB_BASIC
    sfg->SV_6DOF_GB_BASIC.resetflag   = true;
#endif
#if F_6DOF_GY_KALMAN
    sfg->SV_6DOF_GY_KALMAN.resetflag  = true;
#endif
#if F_9DOF_GBY_KALMAN
    sfg->SV_9DOF_GBY_KALMAN.resetflag = true;
#endif

    // reset the loop counter to zero for first iteration
    sfg->loopcounter = 0;
    return;
}

Here is the caller graph for this function:

void fRun_1DOF_P_BASIC	(	struct SV_1DOF_P_BASIC *	pthisSV,
		struct PressureSensor *	pthisPressure
	)

Definition at line 447 of file fusion.c.

Referenced by fFuseSensors().

{
    // if requested, do a reset and return
    if (pthisSV->resetflag)
    {
        fInit_1DOF_P_BASIC(pthisSV, pthisPressure, FLPFSECS_1DOF_P_BASIC);
        return;
    }

    // exponentially low pass filter the pressure and temperature.
    // when executed at (typically) 25Hz, this filter interpolates the raw signals which are
    // output every 1s in Auto Acquisition mode.
    pthisSV->fLPH += pthisSV->flpf * (pthisPressure->fH - pthisSV->fLPH);
    pthisSV->fLPT += pthisSV->flpf * (pthisPressure->fT - pthisSV->fLPT);

    return;
}   // end fRun_1DOF_P_BASIC

Here is the call graph for this function:

Here is the caller graph for this function:

void fRun_3DOF_B_BASIC	(	struct SV_3DOF_B_BASIC *	pthisSV,
		struct MagSensor *	pthisMag
	)

Definition at line 517 of file fusion.c.

Referenced by fFuseSensors().

{
    // if requested, do a reset and return
    if (pthisSV->resetflag)
    {
        fInit_3DOF_B_BASIC(pthisSV, pthisMag, FLPFSECS_3DOF_B_BASIC);
        return;
    }

    // calculate the 3DOF magnetometer orientation matrix from fBc
#if THISCOORDSYSTEM == NED
    f3DOFMagnetometerMatrixNED(pthisSV->fR, pthisMag->fBc);
#elif THISCOORDSYSTEM == ANDROID
    f3DOFMagnetometerMatrixAndroid(pthisSV->fR, pthisMag->fBc);
#else // WIN8
    f3DOFMagnetometerMatrixWin8(pthisSV->fR, pthisMag->fBc);
#endif

    //  compute the instantaneous quaternion and low pass filter
    fQuaternionFromRotationMatrix(pthisSV->fR, &(pthisSV->fq));
    fLPFOrientationQuaternion(&(pthisSV->fq), &(pthisSV->fLPq), pthisSV->flpf,
                              pthisSV->fdeltat, pthisSV->fOmega);

    // compute the low pass rotation matrix and rotation vector from low pass filtered quaternion
    fRotationMatrixFromQuaternion(pthisSV->fLPR, &(pthisSV->fLPq));
    fRotationVectorDegFromQuaternion(&(pthisSV->fLPq), pthisSV->fLPRVec);

    // calculate the Euler angles from the low pass orientation matrix
#if THISCOORDSYSTEM == NED
    fNEDAnglesDegFromRotationMatrix(pthisSV->fLPR, &(pthisSV->fLPPhi),
                                    &(pthisSV->fLPThe), &(pthisSV->fLPPsi),
                                    &(pthisSV->fLPRho), &(pthisSV->fLPChi));
#elif THISCOORDSYSTEM == ANDROID
    fAndroidAnglesDegFromRotationMatrix(pthisSV->fLPR, &(pthisSV->fLPPhi),
                                        &(pthisSV->fLPThe), &(pthisSV->fLPPsi),
                                        &(pthisSV->fLPRho), &(pthisSV->fLPChi));
#else // WIN8
    fWin8AnglesDegFromRotationMatrix(pthisSV->fLPR, &(pthisSV->fLPPhi),
                                     &(pthisSV->fLPThe), &(pthisSV->fLPPsi),
                                     &(pthisSV->fLPRho), &(pthisSV->fLPChi));
#endif
    return;
}

Here is the call graph for this function:

Here is the caller graph for this function:

void fRun_3DOF_G_BASIC	(	struct SV_3DOF_G_BASIC *	pthisSV,
		struct AccelSensor *	pthisAccel
	)

Definition at line 467 of file fusion.c.

Referenced by fFuseSensors().

{
    // if requested, do a reset and return
    if (pthisSV->resetflag)
    {
        fInit_3DOF_G_BASIC(pthisSV, pthisAccel, FLPFSECS_3DOF_G_BASIC);
        return;
    }

    // apply the tilt estimation algorithm to get the instantaneous orientation matrix
#if THISCOORDSYSTEM == NED
    f3DOFTiltNED(pthisSV->fR, pthisAccel->fGc);
#elif THISCOORDSYSTEM == ANDROID
    f3DOFTiltAndroid(pthisSV->fR, pthisAccel->fGc);
#else // WIN8
    f3DOFTiltWin8(pthisSV->fR, pthisAccel->fGc);
#endif

    // compute the instantaneous quaternion and low pass filter
    fQuaternionFromRotationMatrix(pthisSV->fR, &(pthisSV->fq));
    fLPFOrientationQuaternion(&(pthisSV->fq), &(pthisSV->fLPq), pthisSV->flpf,
                              pthisSV->fdeltat, pthisSV->fOmega);

    // compute the low pass rotation matrix and rotation vector from low pass filtered quaternion
    fRotationMatrixFromQuaternion(pthisSV->fLPR, &(pthisSV->fLPq));
    fRotationVectorDegFromQuaternion(&(pthisSV->fLPq), pthisSV->fLPRVec);

    // calculate the Euler angles from the low pass orientation matrix
#if THISCOORDSYSTEM == NED
    fNEDAnglesDegFromRotationMatrix(pthisSV->fLPR, &(pthisSV->fLPPhi),
                                    &(pthisSV->fLPThe), &(pthisSV->fLPPsi),
                                    &(pthisSV->fLPRho), &(pthisSV->fLPChi));
#elif THISCOORDSYSTEM == ANDROID
    fAndroidAnglesDegFromRotationMatrix(pthisSV->fLPR, &(pthisSV->fLPPhi),
                                        &(pthisSV->fLPThe), &(pthisSV->fLPPsi),
                                        &(pthisSV->fLPRho), &(pthisSV->fLPChi));
#else // WIN8
    fWin8AnglesDegFromRotationMatrix(pthisSV->fLPR, &(pthisSV->fLPPhi),
                                     &(pthisSV->fLPThe), &(pthisSV->fLPPsi),
                                     &(pthisSV->fLPRho), &(pthisSV->fLPChi));
#endif

    // force the yaw and compass angles to zero
    pthisSV->fLPPsi = pthisSV->fLPRho = 0.0F;

    return;
}   // end fRun_3DOF_G_BASIC

Here is the call graph for this function:

Here is the caller graph for this function:

void fRun_3DOF_Y_BASIC	(	struct SV_3DOF_Y_BASIC *	pthisSV,
		struct GyroSensor *	pthisGyro
	)

Definition at line 562 of file fusion.c.

Referenced by fFuseSensors().

{
    Quaternion  ftmpq;  // scratch quaternion
    int8        i;      // loop counter

    // if requested, do a reset and return
    if (pthisSV->resetflag)
    {
        fInit_3DOF_Y_BASIC(pthisSV);
        return;
    }

    // perform an approximate gyro integration for this algorithm using the average of all gyro measurments
    // in the FIFO rather than computing the products of the individual quaternions.
    // the reason is this algorithm does not estimate and subtract the gyro offset so any loss of integration accuracy
    // from using the average gyro measurement is irrelevant.
    // calculate the angular velocity (deg/s) and rotation vector from the average measurement
    for (i = CHX; i <= CHZ; i++) pthisSV->fOmega[i] = pthisGyro->fYs[i];

    // compute the incremental rotation quaternion ftmpq, rotate the orientation quaternion fq
    // and re-normalize fq to prevent error propagation
    fQuaternionFromRotationVectorDeg(&ftmpq, pthisSV->fOmega, pthisSV->fdeltat);
    qAeqAxB(&(pthisSV->fq), &ftmpq);
    fqAeqNormqA(&(pthisSV->fq));

    // get the rotation matrix and rotation vector from the orientation quaternion fq
    fRotationMatrixFromQuaternion(pthisSV->fR, &(pthisSV->fq));
    fRotationVectorDegFromQuaternion(&(pthisSV->fq), pthisSV->fRVec);

    // compute the Euler angles from the orientation matrix
#if THISCOORDSYSTEM == NED
    fNEDAnglesDegFromRotationMatrix(pthisSV->fR, &(pthisSV->fPhi),
                                    &(pthisSV->fThe), &(pthisSV->fPsi),
                                    &(pthisSV->fRho), &(pthisSV->fChi));
#elif THISCOORDSYSTEM == ANDROID
    fAndroidAnglesDegFromRotationMatrix(pthisSV->fR, &(pthisSV->fPhi),
                                        &(pthisSV->fThe), &(pthisSV->fPsi),
                                        &(pthisSV->fRho), &(pthisSV->fChi));
#else // WIN8
    fWin8AnglesDegFromRotationMatrix(pthisSV->fR, &(pthisSV->fPhi),
                                     &(pthisSV->fThe), &(pthisSV->fPsi),
                                     &(pthisSV->fRho), &(pthisSV->fChi));
#endif
    return;
}                       // end fRun_3DOF_Y_BASIC

Here is the call graph for this function:

Here is the caller graph for this function:

void fRun_6DOF_GB_BASIC	(	struct SV_6DOF_GB_BASIC *	pthisSV,
		struct MagSensor *	pthisMag,
		struct AccelSensor *	pthisAccel
	)

Definition at line 610 of file fusion.c.

Referenced by fFuseSensors().

{
    float ftmp1, ftmp2, ftmp3, ftmp4;

    // if requested, do a reset and return
    if (pthisSV->resetflag)
    {
        fInit_6DOF_GB_BASIC(pthisSV, pthisAccel, pthisMag,
                            FLPFSECS_6DOF_GB_BASIC);
        return;
    }

    // call the eCompass algorithm to get the instantaneous orientation matrix and inclination angle
#if THISCOORDSYSTEM == NED
    feCompassNED(pthisSV->fR, &(pthisSV->fDelta), &ftmp1, &ftmp2, pthisMag->fBc, pthisAccel->fGc, &ftmp3, &ftmp4);
#elif THISCOORDSYSTEM == ANDROID
    feCompassAndroid(pthisSV->fR, &(pthisSV->fDelta), &ftmp1, &ftmp2, pthisMag->fBc, pthisAccel->fGc, &ftmp3, &ftmp4);
#else  // WIN8
    feCompassWin8(pthisSV->fR, &(pthisSV->fDelta), &ftmp1, &ftmp2, pthisMag->fBc, pthisAccel->fGc, &ftmp3, &ftmp4);
#endif

    //  compute the instantaneous quaternion and low pass filter
    fQuaternionFromRotationMatrix(pthisSV->fR, &(pthisSV->fq));
    fLPFOrientationQuaternion(&(pthisSV->fq), &(pthisSV->fLPq), pthisSV->flpf,
                              pthisSV->fdeltat, pthisSV->fOmega);

    // compute the low pass rotation matrix and rotation vector from low pass filtered quaternion
    fRotationMatrixFromQuaternion(pthisSV->fLPR, &(pthisSV->fLPq));
    fRotationVectorDegFromQuaternion(&(pthisSV->fLPq), pthisSV->fLPRVec);

    // calculate the Euler angles from the low pass orientation matrix
#if THISCOORDSYSTEM == NED
    fNEDAnglesDegFromRotationMatrix(pthisSV->fLPR, &(pthisSV->fLPPhi),
                                    &(pthisSV->fLPThe), &(pthisSV->fLPPsi),
                                    &(pthisSV->fLPRho), &(pthisSV->fLPChi));
#elif THISCOORDSYSTEM == ANDROID
    fAndroidAnglesDegFromRotationMatrix(pthisSV->fLPR, &(pthisSV->fLPPhi),
                                        &(pthisSV->fLPThe), &(pthisSV->fLPPsi),
                                        &(pthisSV->fLPRho), &(pthisSV->fLPChi));
#else // WIN8
    fWin8AnglesDegFromRotationMatrix(pthisSV->fLPR, &(pthisSV->fLPPhi),
                                     &(pthisSV->fLPThe), &(pthisSV->fLPPsi),
                                     &(pthisSV->fLPRho), &(pthisSV->fLPChi));
#endif

    // low pass filter the geomagnetic inclination angle with a simple exponential filter
    pthisSV->fLPDelta += pthisSV->flpf * (pthisSV->fDelta - pthisSV->fLPDelta);

    return;
}   // end fRun_6DOF_GB_BASIC

Here is the call graph for this function:

Here is the caller graph for this function:

void fRun_6DOF_GY_KALMAN	(	struct SV_6DOF_GY_KALMAN *	pthisSV,
		struct AccelSensor *	pthisAccel,
		struct GyroSensor *	pthisGyro
	)

Definition at line 663 of file fusion.c.

Referenced by fFuseSensors().

{
    // local scalars and arrays
    float       ftmpMi3x1[3];       // temporary vector used for a priori calculations
    float       ftmp3DOF3x1[3];     // temporary vector used for 3DOF calculations
    float       ftmpA3x3[3][3];     // scratch 3x3 matrix
    float       fQvGQa;             // accelerometer noise covariance to 1g sphere
    float       fC3x6ik;            // element i, k of measurement matrix C
    float       fC3x6jk;            // element j, k of measurement matrix C
    float       fmodGc;             // modulus of fGc[]
    Quaternion  fqMi;               // a priori orientation quaternion
    Quaternion  ftmpq;              // scratch quaternion
    float       ftmp;               // scratch float
    int8        ierror;             // matrix inversion error flag
    int8        i,
                j,
                k;                  // loop counters

    // working arrays for 3x3 matrix inversion
    float       *pfRows[3];
    int8        iColInd[3];
    int8        iRowInd[3];
    int8        iPivot[3];

    // if requested, do a reset initialization with no further processing
    if (pthisSV->resetflag)
    {
        fInit_6DOF_GY_KALMAN(pthisSV, pthisAccel, pthisGyro);
        return;
    }

    // compute the average angular velocity (used for display only) from the average measurement minus gyro offset
    for (i = CHX; i <= CHZ; i++)
        pthisSV->fOmega[i] = (float) pthisGyro->iYs[i] *
            pthisGyro->fDegPerSecPerCount -
            pthisSV->fbPl[i];

    // initialize the a priori orientation quaternion fqMi to the previous iteration's a posteriori estimate
    // and incrementally rotate fqMi by the contents of the gyro FIFO buffer
    fqMi = pthisSV->fqPl;
    if (pthisGyro->iFIFOCount > 0)
    {
        // set ftmp to the interval between the FIFO gyro measurements
        ftmp = pthisSV->fdeltat / (float) pthisGyro->iFIFOCount;

        // normal case, loop over all the buffered gyroscope measurements
        for (j = 0; j < pthisGyro->iFIFOCount; j++)
        {
            // calculate the instantaneous angular velocity subtracting the gyro offset
            for (i = CHX; i <= CHZ; i++)
                ftmpMi3x1[i] = (float) pthisGyro->iYsFIFO[j][i] *
                    pthisGyro->fDegPerSecPerCount -
                    pthisSV->fbPl[i];

            // compute the incremental rotation quaternion ftmpq and integrate the a priori orientation quaternion fqMi
            fQuaternionFromRotationVectorDeg(&ftmpq, ftmpMi3x1, ftmp);
            qAeqAxB(&fqMi, &ftmpq);
        }
    }
    else
    {
        // special case with no new FIFO measurements, use the previous iteration's average gyro reading to compute
        // the incremental rotation quaternion ftmpq and integrate the a priori orientation quaternion fqMi
        fQuaternionFromRotationVectorDeg(&ftmpq, pthisSV->fOmega,
                                         pthisSV->fdeltat);
        qAeqAxB(&fqMi, &ftmpq);
    }

    // set ftmp3DOF3x1 to the 3DOF gravity vector in the sensor frame
    fmodGc = sqrtf(fabs(pthisAccel->fGc[CHX] * pthisAccel->fGc[CHX] +
                   pthisAccel->fGc[CHY] * pthisAccel->fGc[CHY] +
                   pthisAccel->fGc[CHZ] * pthisAccel->fGc[CHZ]));
    if (fmodGc != 0.0F)
    {
        // normal non-freefall case
        ftmp = 1.0F / fmodGc;
        ftmp3DOF3x1[CHX] = pthisAccel->fGc[CHX] * ftmp;
        ftmp3DOF3x1[CHY] = pthisAccel->fGc[CHY] * ftmp;
        ftmp3DOF3x1[CHZ] = pthisAccel->fGc[CHZ] * ftmp;
    }
    else
    {
        // use zero tilt in case of freefall
        ftmp3DOF3x1[CHX] = 0.0F;
        ftmp3DOF3x1[CHY] = 0.0F;
        ftmp3DOF3x1[CHZ] = 1.0F;
    }

    // correct accelerometer gravity vector for different coordinate systems
#if THISCOORDSYSTEM == NED
    // +1g in accelerometer z axis (z down) when PCB is flat so no correction needed
#elif THISCOORDSYSTEM == ANDROID
    // +1g in accelerometer z axis (z up) when PCB is flat so negate the vector to obtain gravity
    ftmp3DOF3x1[CHX] = -ftmp3DOF3x1[CHX];
    ftmp3DOF3x1[CHY] = -ftmp3DOF3x1[CHY];
    ftmp3DOF3x1[CHZ] = -ftmp3DOF3x1[CHZ];
#else // WIN8
    // -1g in accelerometer z axis (z up) when PCB is flat so no correction needed
#endif

    // set ftmpMi3x1 to the a priori gravity vector in the sensor frame from the a priori quaternion
    ftmpMi3x1[CHX] = 2.0F * (fqMi.q1 * fqMi.q3 - fqMi.q0 * fqMi.q2);
    ftmpMi3x1[CHY] = 2.0F * (fqMi.q2 * fqMi.q3 + fqMi.q0 * fqMi.q1);
    ftmpMi3x1[CHZ] = 2.0F * (fqMi.q0 * fqMi.q0 + fqMi.q3 * fqMi.q3) - 1.0F;

    // correct a priori gravity vector for different coordinate systems
#if THISCOORDSYSTEM == NED
    // z axis is down (NED) so no correction needed
#else // ANDROID and WIN8
    // z axis is up (ANDROID and WIN8 ENU) so no negate the vector to obtain gravity
    ftmpMi3x1[CHX] = -ftmpMi3x1[CHX];
    ftmpMi3x1[CHY] = -ftmpMi3x1[CHY];
    ftmpMi3x1[CHZ] = -ftmpMi3x1[CHZ];
#endif

    // calculate the rotation quaternion between 3DOF and a priori gravity vectors (ignored minus signs cancel here)
    // and copy the quaternion vector components to the measurement error vector fZErr
    fveqconjgquq(&ftmpq, ftmp3DOF3x1, ftmpMi3x1);
    pthisSV->fZErr[CHX] = ftmpq.q1;
    pthisSV->fZErr[CHY] = ftmpq.q2;
    pthisSV->fZErr[CHZ] = ftmpq.q3;

    // update Qw using the a posteriori error vectors from the previous iteration.
    // as Qv increases or Qw decreases, K -> 0 and the Kalman filter is weighted towards the a priori prediction
    // as Qv decreases or Qw increases, KC -> I and the Kalman filter is weighted towards the measurement.
    // initialize Qw to all zeroes
    for (i = 0; i < 6; i++)
        for (j = 0; j < 6; j++) pthisSV->fQw6x6[i][j] = 0.0F;

    // partial diagonal gyro offset terms
    pthisSV->fQw6x6[3][3] = pthisSV->fbErrPl[CHX] * pthisSV->fbErrPl[CHX];
    pthisSV->fQw6x6[4][4] = pthisSV->fbErrPl[CHY] * pthisSV->fbErrPl[CHY];
    pthisSV->fQw6x6[5][5] = pthisSV->fbErrPl[CHZ] * pthisSV->fbErrPl[CHZ];

    // diagonal gravity vector components
    pthisSV->fQw6x6[0][0] = pthisSV->fqgErrPl[CHX] *
        pthisSV->fqgErrPl[CHX] +
        pthisSV->fAlphaSqQvYQwbOver12 +
        pthisSV->fAlphaSqOver4 *
        pthisSV->fQw6x6[3][3];
    pthisSV->fQw6x6[1][1] = pthisSV->fqgErrPl[CHY] *
        pthisSV->fqgErrPl[CHY] +
        pthisSV->fAlphaSqQvYQwbOver12 +
        pthisSV->fAlphaSqOver4 *
        pthisSV->fQw6x6[4][4];
    pthisSV->fQw6x6[2][2] = pthisSV->fqgErrPl[CHZ] *
        pthisSV->fqgErrPl[CHZ] +
        pthisSV->fAlphaSqQvYQwbOver12 +
        pthisSV->fAlphaSqOver4 *
        pthisSV->fQw6x6[5][5];

    // remaining diagonal gyro offset components
    pthisSV->fQw6x6[3][3] += pthisSV->fQwbOver3;
    pthisSV->fQw6x6[4][4] += pthisSV->fQwbOver3;
    pthisSV->fQw6x6[5][5] += pthisSV->fQwbOver3;

    // off diagonal gravity and gyro offset components
    pthisSV->fQw6x6[0][3] = pthisSV->fQw6x6[3][0] = pthisSV->fqgErrPl[CHX] *
        pthisSV->fbErrPl[CHX] -
        pthisSV->fAlphaOver2 *
        pthisSV->fQw6x6[3][3];
    pthisSV->fQw6x6[1][4] = pthisSV->fQw6x6[4][1] = pthisSV->fqgErrPl[CHY] *
        pthisSV->fbErrPl[CHY] -
        pthisSV->fAlphaOver2 *
        pthisSV->fQw6x6[4][4];
    pthisSV->fQw6x6[2][5] = pthisSV->fQw6x6[5][2] = pthisSV->fqgErrPl[CHZ] *
        pthisSV->fbErrPl[CHZ] -
        pthisSV->fAlphaOver2 *
        pthisSV->fQw6x6[5][5];

    // calculate the vector fQv containing the diagonal elements of the measurement covariance matrix Qv
    ftmp = fmodGc - 1.0F;
    fQvGQa = 3.0F * ftmp * ftmp;
    if (fQvGQa < FQVG_6DOF_GY_KALMAN) fQvGQa = FQVG_6DOF_GY_KALMAN;
    pthisSV->fQv = ONEOVER12 * ftmp * ftmp + pthisSV->fAlphaSqQvYQwbOver12;

    // calculate the 6x3 Kalman gain matrix K = Qw * C^T * inv(C * Qw * C^T + Qv)
    // set fQwCT6x3 = Qw.C^T where Qw has size 6x6 and C^T has size 6x3
    for (i = 0; i < 6; i++)         // loop over rows
    {
        for (j = 0; j < 3; j++)     // loop over columns
        {
            pthisSV->fQwCT6x3[i][j] = 0.0F;

            // accumulate matrix sum
            for (k = 0; k < 6; k++)
            {
                // determine fC3x6[j][k] since the matrix is highly sparse
                fC3x6jk = 0.0F;
                if (k == j) fC3x6jk = 1.0F;
                if (k == (j + 3)) fC3x6jk = -pthisSV->fAlphaOver2;

                // accumulate fQwCT6x3[i][j] += Qw6x6[i][k] * C[j][k]
                if ((pthisSV->fQw6x6[i][k] != 0.0F) && (fC3x6jk != 0.0F))
                {
                    if (fC3x6jk == 1.0F)
                        pthisSV->fQwCT6x3[i][j] += pthisSV->fQw6x6[i][k];
                    else
                        pthisSV->fQwCT6x3[i][j] += pthisSV->fQw6x6[i][k] * fC3x6jk;
                }
            }
        }
    }

    // set symmetric ftmpA3x3 = C.(Qw.C^T) + Qv = C.fQwCT6x3 + Qv
    for (i = 0; i < 3; i++)         // loop over rows
    {
        for (j = i; j < 3; j++)     // loop over on and above diagonal columns
        {
            // zero off diagonal and set diagonal to Qv
            if (i == j)
                ftmpA3x3[i][j] = pthisSV->fQv;
            else
                ftmpA3x3[i][j] = 0.0F;

            // accumulate matrix sum
            for (k = 0; k < 6; k++)
            {
                // determine fC3x6[i][k]
                fC3x6ik = 0.0F;
                if (k == i) fC3x6ik = 1.0F;
                if (k == (i + 3)) fC3x6ik = -pthisSV->fAlphaOver2;

                // accumulate ftmpA3x3[i][j] += C[i][k] & fQwCT6x3[k][j]
                if ((fC3x6ik != 0.0F) && (pthisSV->fQwCT6x3[k][j] != 0.0F))
                {
                    if (fC3x6ik == 1.0F)
                        ftmpA3x3[i][j] += pthisSV->fQwCT6x3[k][j];
                    else
                        ftmpA3x3[i][j] += fC3x6ik * pthisSV->fQwCT6x3[k][j];
                }
            }
        }
    }

    // set ftmpA3x3 below diagonal elements to above diagonal elements
    ftmpA3x3[1][0] = ftmpA3x3[0][1];
    ftmpA3x3[2][0] = ftmpA3x3[0][2];
    ftmpA3x3[2][1] = ftmpA3x3[1][2];

    // invert ftmpA3x3 in situ to give ftmpA3x3 = inv(C * Qw * C^T + Qv) = inv(ftmpA3x3)
    for (i = 0; i < 3; i++) pfRows[i] = ftmpA3x3[i];
    fmatrixAeqInvA(pfRows, iColInd, iRowInd, iPivot, 3, &ierror);

    // on successful inversion set Kalman gain matrix fK6x3 = Qw * C^T * inv(C * Qw * C^T + Qv) = fQwCT6x3 * ftmpA3x3
    if (!ierror)
    {
        // normal case
        for (i = 0; i < 6; i++)     // loop over rows
        {
            for (j = 0; j < 3; j++) // loop over columns
            {
                pthisSV->fK6x3[i][j] = 0.0F;
                for (k = 0; k < 3; k++)
                {
                    if ((pthisSV->fQwCT6x3[i][k] != 0.0F) &&
                        (ftmpA3x3[k][j] != 0.0F))
                    {
                        pthisSV->fK6x3[i][j] += pthisSV->fQwCT6x3[i][k] * ftmpA3x3[k][j];
                    }
                }
            }
        }
    }
    else
    {
        // ftmpA3x3 was singular so set Kalman gain matrix fK6x3 to zero
        for (i = 0; i < 6; i++)     // loop over rows
        {
            for (j = 0; j < 3; j++) // loop over columns
            {
                pthisSV->fK6x3[i][j] = 0.0F;
            }
        }
    }

    // calculate the a posteriori gravity and geomagnetic tilt quaternion errors and gyro offset error vector
    // from the Kalman matrix fK6x3 and from the measurement error vector fZErr.
    for (i = CHX; i <= CHZ; i++)
    {
        // gravity tilt vector error component
        if (pthisSV->fK6x3[i][CHX] != 0.0F)
            pthisSV->fqgErrPl[i] = pthisSV->fK6x3[i][CHX] * pthisSV->fZErr[CHX];
        else
            pthisSV->fqgErrPl[i] = 0.0F;
        if (pthisSV->fK6x3[i][CHY] != 0.0F)
            pthisSV->fqgErrPl[i] += pthisSV->fK6x3[i][CHY] * pthisSV->fZErr[CHY];
        if (pthisSV->fK6x3[i][CHZ] != 0.0F)
            pthisSV->fqgErrPl[i] += pthisSV->fK6x3[i][CHZ] * pthisSV->fZErr[CHZ];

        // gyro offset vector error component
        if (pthisSV->fK6x3[i + 3][CHX] != 0.0F)
            pthisSV->fbErrPl[i] = pthisSV->fK6x3[i + 3][CHX] * pthisSV->fZErr[CHX];
        else
            pthisSV->fbErrPl[i] = 0.0F;
        if (pthisSV->fK6x3[i + 3][CHY] != 0.0F)
            pthisSV->fbErrPl[i] += pthisSV->fK6x3[i + 3][CHY] * pthisSV->fZErr[CHY];
        if (pthisSV->fK6x3[i + 3][CHZ] != 0.0F)
            pthisSV->fbErrPl[i] += pthisSV->fK6x3[i + 3][CHZ] * pthisSV->fZErr[CHZ];
    }

    // set ftmpq to the gravity tilt correction (conjugate) quaternion
    ftmpq.q1 = -pthisSV->fqgErrPl[CHX];
    ftmpq.q2 = -pthisSV->fqgErrPl[CHY];
    ftmpq.q3 = -pthisSV->fqgErrPl[CHZ];
    ftmp = ftmpq.q1 * ftmpq.q1 + ftmpq.q2 * ftmpq.q2 + ftmpq.q3 * ftmpq.q3;

    // determine the scalar component q0 to enforce normalization
    if (ftmp <= 1.0F)
    {
        // normal case
        ftmpq.q0 = sqrtf(fabsf(1.0F - ftmp));
    }
    else
    {
        // if vector component exceeds unity then set to 180 degree rotation and force normalization
        ftmp = 1.0F / sqrtf(ftmp);
        ftmpq.q0 = 0.0F;
        ftmpq.q1 *= ftmp;
        ftmpq.q2 *= ftmp;
        ftmpq.q3 *= ftmp;
    }

    // apply the gravity tilt correction quaternion so fqPl = fqMi.(fqgErrPl)* = fqMi.ftmpq and normalize
    qAeqBxC(&(pthisSV->fqPl), &fqMi, &ftmpq);

    // normalize the a posteriori quaternion and compute the a posteriori rotation matrix and rotation vector
    fqAeqNormqA(&(pthisSV->fqPl));
    fRotationMatrixFromQuaternion(pthisSV->fRPl, &(pthisSV->fqPl));
    fRotationVectorDegFromQuaternion(&(pthisSV->fqPl), pthisSV->fRVecPl);

    // update the a posteriori gyro offset vector: b+[k] = b-[k] - be+[k] = b+[k] - be+[k] (deg/s)
    // limiting the correction to the maximum permitted by the random walk model
    for (i = CHX; i <= CHZ; i++)
    {
        if (pthisSV->fbErrPl[i] > pthisSV->fMaxGyroOffsetChange)
            pthisSV->fbPl[i] -= pthisSV->fMaxGyroOffsetChange;
        else if (pthisSV->fbErrPl[i] < -pthisSV->fMaxGyroOffsetChange)
            pthisSV->fbPl[i] += pthisSV->fMaxGyroOffsetChange;
        else
            pthisSV->fbPl[i] -= pthisSV->fbErrPl[i];
    }

    // compute the linear acceleration fAccGl in the global frame
    // first de-rotate the accelerometer measurement fGc from the sensor to global frame
    // using the transpose (inverse) of the orientation matrix fRPl
    fveqRu(pthisSV->fAccGl, pthisSV->fRPl, pthisAccel->fGc, 1);

    // sutract the fixed gravity vector in the global frame leaving linear acceleration
#if THISCOORDSYSTEM == NED
    // gravity positive NED
    pthisSV->fAccGl[CHX] = -pthisSV->fAccGl[CHX];
    pthisSV->fAccGl[CHY] = -pthisSV->fAccGl[CHY];
    pthisSV->fAccGl[CHZ] = -(pthisSV->fAccGl[CHZ] - 1.0F);
#elif THISCOORDSYSTEM == ANDROID
    // acceleration positive ENU
    pthisSV->fAccGl[CHZ] = pthisSV->fAccGl[CHZ] - 1.0F;
#else // WIN8
    // gravity positive ENU
    pthisSV->fAccGl[CHX] = -pthisSV->fAccGl[CHX];
    pthisSV->fAccGl[CHY] = -pthisSV->fAccGl[CHY];
    pthisSV->fAccGl[CHZ] = -(pthisSV->fAccGl[CHZ] + 1.0F);
#endif

    // compute the a posteriori Euler angles from the a posteriori orientation matrix fRPl
#if THISCOORDSYSTEM == NED
    fNEDAnglesDegFromRotationMatrix(pthisSV->fRPl, &(pthisSV->fPhiPl),
                                    &(pthisSV->fThePl), &(pthisSV->fPsiPl),
                                    &(pthisSV->fRhoPl), &(pthisSV->fChiPl));
#elif THISCOORDSYSTEM == ANDROID
    fAndroidAnglesDegFromRotationMatrix(pthisSV->fRPl, &(pthisSV->fPhiPl),
                                        &(pthisSV->fThePl), &(pthisSV->fPsiPl),
                                        &(pthisSV->fRhoPl), &(pthisSV->fChiPl));
#else // WIN8
    fWin8AnglesDegFromRotationMatrix(pthisSV->fRPl, &(pthisSV->fPhiPl),
                                     &(pthisSV->fThePl), &(pthisSV->fPsiPl),
                                     &(pthisSV->fRhoPl), &(pthisSV->fChiPl));
#endif
    return;
}   // end fRun_6DOF_GY_KALMAN

Here is the call graph for this function:

Here is the caller graph for this function: